#include <string.h>
#include <ServoSoft.h>
#include <MsTimer2.h>
//#include <stdlib.h>
#include <stdio.h>
#include <defArduinoParaArduino.h>

#define DEBUG 0
#define ELETROIMA 13
#define TOKE_EF_BAIXO 10
#define TOKE_EF_CIMA 12
#define TOKE_RABETA 5
#define ENABLE_Z 11

#define ZERO_ServoZ 95

/* Posicao inicial do scara (X=17,Y=0,ef=0) */
#define THETA1_INICIAL 119
#define THETA2_INICIAL 37
#define THETA_EF_INICIAL 133

#include "WProgram.h"
int serialGetMsg();
void refreshServos();
void Z_cima( int time_cs );
void Z_baixo( int time_cs );
void deixaBloco( );
void pegaBloco( );
void leSensores();
int mudaServos(  );
int pegaInstrucao();
void setup();
void loop();
int theta1_anterior = THETA1_INICIAL;
int theta2_anterior = THETA2_INICIAL;
int theta_ef_anterior = THETA_EF_INICIAL;

int fuck=1;
int theta1 = THETA1_INICIAL;
int theta2 = THETA2_INICIAL;
int theta_ef = THETA_EF_INICIAL;
 
char msg[16];
int msgSize;
char response[16];
int numSeqEsperado = 1;
short int start = 0;
short int executarTarefa = 0;
short int posicaoAlcancada = 0;
volatile int reboot = LOW;
volatile int esperaStart = HIGH;


 Servo junta01;
 Servo junta02;
 Servo efetuador;
 Servo servoZ;

int serialGetMsg()
{
  int i;  
  i=0;
  
    while (Serial.available())
        msg[i++] = Serial.read();
    
  if(i>0)
  {
    msg[i]='\0';
    Serial.flush(); // prestar atencao se isso nao ta dando pau!
  }
  
  msgSize=i;
  
  return i;
}


void refreshServos()
{
  
  int n=0;
  int numSeqRecebido = -1;
  
  if( theta1 > theta1_anterior )
    junta01.write(++theta1_anterior);
  else if( theta1 < theta1_anterior )
          junta01.write(--theta1_anterior);
       
  if( theta2 > theta2_anterior )
    junta02.write(++theta2_anterior);
  else if( theta2 < theta2_anterior )
          junta02.write(--theta2_anterior);
       
  if( theta_ef > theta_ef_anterior )
    efetuador.write(++theta_ef_anterior);
  else if( theta_ef < theta_ef_anterior )
          efetuador.write(--theta_ef_anterior); 
  
  Servo::refresh();
  
  if( (theta1 == theta1_anterior) && (theta2 == theta2_anterior) && (theta_ef == theta_ef_anterior) )
    posicaoAlcancada = 1;
  else
    posicaoAlcancada = 0;
    
      
  if( start != 0 )
    n = serialGetMsg();
  
  if( n )
  {
    if( strlen(msg) > 8 )
    {
       if( (msg[0]=='<') && (msg[11]=='>') )
       {
         numSeqRecebido = (msg[10]-'0'); 
         
              if( numSeqRecebido == numSeqEsperado )
                executarTarefa = 1;
              else  
                Serial.println("NACK");  
       }
       else
          Serial.println("NACK");
    }
    else
    {    
      if( (msg[0]=='<') && (msg[6]=='>') && (strlen(msg) == 7) )
      {
        numSeqRecebido = (msg[5]-'0'); 
            
              if( numSeqRecebido == numSeqEsperado )
                executarTarefa = 1;
              else  
                Serial.println("NACK");  
      }
      else
      { 
        
        if( strcmp(msg,"[START]") == 0 )
          { 
            numSeqEsperado = 1;
            executarTarefa = 0;
            Serial.println("ACK");
            //asm volatile ("jmp 0"); // reset via software =)
          }
          else
            Serial.println("NACK");
      }
    }
  }
  
}



void Z_cima( int time_cs ) // 10^-2 seg
{
  digitalWrite(ENABLE_Z,HIGH);
  
  if( time_cs == ATE_TOKE )
  {
    servoZ.write(ZERO_ServoZ-5);
    
    while( digitalRead(TOKE_EF_CIMA) != 1 )
    { delay(5); }
    
    servoZ.write(ZERO_ServoZ);
  }
  else
  {
    servoZ.write(ZERO_ServoZ-5);
    delay(time_cs*100);
    servoZ.write(ZERO_ServoZ);
  }

digitalWrite(ENABLE_Z,LOW);  
}



void Z_baixo( int time_cs )
{
  digitalWrite(ENABLE_Z,HIGH);
  
   if( time_cs == ATE_TOKE )
  {
    servoZ.write(ZERO_ServoZ+3);
    
    while( digitalRead(TOKE_EF_BAIXO) != 1 )
    { delay(5); }
    
    servoZ.write(ZERO_ServoZ);
  }
  else
  {
    servoZ.write(ZERO_ServoZ+3);
    delay(time_cs*100);
    servoZ.write(ZERO_ServoZ);
  }

digitalWrite(ENABLE_Z,LOW);  
}




void deixaBloco( )
{
  // desce ate bater
  servoZ.write(ZERO_ServoZ+3);
  digitalWrite(ENABLE_Z,HIGH);
 
  delay(1000); 
  
  while( digitalRead(TOKE_RABETA) != 1 ) 
    delay(3);
    
  servoZ.write(ZERO_ServoZ);
  digitalWrite(ENABLE_Z,LOW);
  digitalWrite(ELETROIMA,HIGH); //desliga (o lenhador inverteu essa porra)
  
  delay(500);
  
  //sobe ate o maximo
  servoZ.write(ZERO_ServoZ-6);
  digitalWrite(ENABLE_Z,HIGH);
  
  while( digitalRead(TOKE_EF_CIMA) != 1 )
   delay(3);
  
  servoZ.write(ZERO_ServoZ);
  digitalWrite(ENABLE_Z,LOW);
}
  



void pegaBloco( )
{
  servoZ.write(ZERO_ServoZ+4);
  digitalWrite(ENABLE_Z,HIGH);
  
  while( digitalRead(TOKE_EF_BAIXO) != 1 )
    delay(3);
  
  delay(50); // da uma "socada" no bloco
  servoZ.write(ZERO_ServoZ);
  digitalWrite(ENABLE_Z,LOW);
  
  digitalWrite(ELETROIMA,LOW); //liga
  
  delay(2000);
  
  servoZ.write(ZERO_ServoZ-6);
  digitalWrite(ENABLE_Z,HIGH);
  
  while( digitalRead(TOKE_EF_CIMA) != 1 )
    delay(3);
  
  servoZ.write(ZERO_ServoZ);
  digitalWrite(ENABLE_Z,LOW);
}


void leSensores()
{

char resposta[10];
char aux[5];
aux[0]= '\0';

int ang = 1023; // ang eh o resultado de uma leitura de sensor!

resposta[0] = '<';
resposta[1] = '\0';

sprintf(aux,"%d",ang); // transforma o int em string
    
if ( strlen(aux) == 1 )
{
  strcpy(resposta,"<000");
  strcat(resposta,aux);
  
} else 
    if ( strlen(aux) == 2 )
    {
       strcpy(resposta,"<00");
       strcat(resposta,aux);
    }
    else 
      if( strlen(aux) == 3 )
      {
        strcpy(resposta,"<0");
        strcat(resposta,aux);
      }
      else
        strcat(resposta,aux);
        
 resposta[5] = '>';
 resposta[6] = '\0';

  Serial.println(resposta);
  
}


int mudaServos(  )
{
  int angulo1 = 999,angulo2 = 999,angulo3 = 999;
  int n = 0;
  
  n = msg[1] - '0';
  angulo1 = 100*n;
  n = msg[2] - '0';
  angulo1 = angulo1 + 10*n;
  n = msg[3] - '0';
  angulo1 = angulo1 + 1*n;
  
  n = msg[4] - '0';
  angulo2 = 100*n;
  n = msg[5] - '0';
  angulo2 = angulo2 + 10*n;
  n = msg[6] - '0';
  angulo2 = angulo2 + 1*n;
  
  n = msg[7] - '0';
  angulo3 = 100*n;
  n = msg[8] - '0';
  angulo3 = angulo3 + 10*n;
  n = msg[9] - '0';
  angulo3 = angulo3 + 1*n;
  
  if( (angulo1 >=0 && angulo1 <=180) && (angulo2 >=0 && angulo2 <=180) && (angulo3 >=0 && angulo3 <=180))
  {
    theta1 = angulo1;
    theta2 = angulo2;
    theta_ef = angulo3;
  }
  else
    return -1;

  delay(50); // pra garantir q o timer vai ver a mudanca

  while( posicaoAlcancada == 0 )
    delay(10);  
  
return 0;    
}



int pegaInstrucao()
{

  long int arg = 0;

  if( strlen(msg) == 7 )
  {
    arg = (msg[1] - '0')*100;
    arg = arg + (msg[2] - '0')*10;
    arg = arg + (msg[3] - '0')*1;
  }
  else
    if( strlen(msg) > 8 )
    {
      if( mudaServos() == -1 ) // atualiza juntas
        return -1;
      else
        return 0;
    }

  if( arg < 0 || arg > 999 ) // soh pra garantir
    return -1;

              switch( msg[4] )
              {
                             
                  case SCARA_PEGA_BLOCO: pegaBloco( );
                                         break;  
                            
        
                  case SCARA_DEIXA_BLOCO: deixaBloco( );
                                          break;                      
        
                  case SCARA_DESCE_EF: Z_baixo( arg );
                                       break; 
        
                  case SCARA_SOBE_EF: Z_cima( arg );
                                       break; 
        
                  case SCARA_SOBE_EF_ATE_TOKE: Z_cima( ATE_TOKE );          
                                               break;
                                               
                  case SCARA_DESCE_EF_ATE_TOKE: Z_baixo( ATE_TOKE );          
                                                break;                             

                  case LE_SENSORES: leSensores();
                                          break;
                                          
                  case SCARA_DETACH_SERVOS: junta01.detach();
                                            junta02.detach();  
                                            efetuador.detach();
                                            servoZ.detach();
                                            break;
        
                 default : return -1;
      
              }

return 0;
}


void setup() 
{ 
  
  MsTimer2::set(20,refreshServos);
  //MsTimer2::set(577,checkSerial);
  Serial.begin(115200);
  
  pinMode(TOKE_EF_BAIXO,INPUT);
  pinMode(TOKE_EF_CIMA,INPUT);
  pinMode(TOKE_RABETA,INPUT);
  pinMode(ELETROIMA,OUTPUT);
  pinMode(ENABLE_Z,OUTPUT);
  
  
  /* valores para o servo TowardPro MG946R */
  junta01.attach(7); 
  junta01.setMinimumPulse(890); 
  junta01.setMaximumPulse(2300);
  
  
  /* valores para o servo TowardPro MG946R */
  junta02.attach(6);
  junta02.setMinimumPulse(820); 
  junta02.setMaximumPulse(2200);
  
  
  /* Valores ajustados para o servinho preto */
  efetuador.attach(8);
  efetuador.setMinimumPulse(530);
  efetuador.setMaximumPulse(2330);
  
  /* Esse servo eh CONTINUO */
  servoZ.attach(9);
  servoZ.setMinimumPulse(900);
  servoZ.setMaximumPulse(2000);

  digitalWrite(ENABLE_Z,LOW);
  digitalWrite(ELETROIMA,HIGH); //desligado    
  
  MsTimer2::start();
  
  delay(100);
} 
 



void loop() 
{
  
  if( fuck == 1) // soh entra uma vez nessa porra
  {

    Serial.println("REBOOT!");
    Serial.println("REBOOT!");
    
    while( start==0 ) // fica esperando o botao de start ser acionado
    { 
      delay(1000);
      start = digitalRead(2);
    }

   junta01.write(THETA1_INICIAL);
   junta02.write(THETA2_INICIAL); 
   efetuador.write(THETA_EF_INICIAL);
   Z_cima( ATE_TOKE );
  
    Serial.println("[BEGIN]"); // duas vezes pra garantir (ja q nao tem ACK)
    Serial.println("[BEGIN]");
    
    fuck=0;  
  }

    delay(10);
  
    
    if( executarTarefa == 1 )            
    {
      if( pegaInstrucao( ) == 0 ) // executa
      {
        Serial.println("ACK");
                 
        if( numSeqEsperado == 9)
          numSeqEsperado = 1;
        else
          numSeqEsperado++;
      }
      else
        Serial.println("NACK");

    executarTarefa = 0;        
    }   
  
      
  if( DEBUG )
  { 
    Serial.println("Angulos das juntas:");
    Serial.print("theta1: ");
    Serial.println(theta1);
    Serial.print("theta2: ");
    Serial.println(theta2);
    Serial.print("theta_ef: ");
    Serial.println(theta_ef);
    
    delay(1000);
   }
 
} 

int main(void)
{
	init();

	setup();
    
	for (;;)
		loop();
        
	return 0;
}

